# 使用准静态以及部分姿态假设，结合关节角度，计算关节扭矩
import sys
import math
from os.path import dirname, join
sys.path.append(join(dirname(__file__), "src"))
from src import *

import csv
header = ['human_index','torque_left','torque_right','support_force_left','support_force_right']
csv_file_path = './mini_calculate.csv'

gravity = 9.8
human_config = Human_Config()
with open(csv_file_path, 'w') as f:
    writer = csv.writer(f)
    writer.writerow(header)
    for i in range(1,human_config.num_row()):
        human_config.get_config(row=i)
        human_index = human_config.human_index
        TotalMass = human_config.mass
        H = human_config.height
        

        # 以下为基础计算方法
        linkMass["UpperLeg_mass"] = 0.1447 * TotalMass
        linkMass["LowerLeg_mass"] = 0.0457 * TotalMass

        linkDimensions["UpperLeg"]["Z"] = 0.245 * H
        linkDimensions["LowerLeg"]["Z"] = 0.246 * H

        m_up = TotalMass - linkMass['LowerLeg_mass']*2


        human_config.get_angle(row = i)
        torque_left = linkDimensions['UpperLeg']['Z']*m_up*gravity*math.cos(math.radians(human_config.left_angle/3))/2
        torque_right = linkDimensions['UpperLeg']['Z']*m_up*gravity*math.cos(math.radians(human_config.right_angle/3))/2
        support_left = m_up*gravity*(torque_left/(torque_right+torque_left))
        support_right = m_up*gravity*(torque_right/(torque_right+torque_left))
        writer.writerow([human_index, torque_left, torque_right, support_left, support_right])
        print(human_index, torque_left, torque_right, support_left, support_right)

    